import("System")
import("System.IO.Ports")
import("Jodell.Tool", "Jodell.Tool")

--打开串口
COMID = "COM16"
Slave_ID = 0

client = Lua_ModbusRtuClient()
client.PortName = COMID
client:Connect()

-- 从设备夹爪数量
Slave_Mc_Num = 4

Device_Save_Addr = 0x0DAC

Asr_Ctrl_Type_Addr = 0x0C1F
Apr_Ctrl_Type_Addr = 0x0C20
Asr_Kp_Addr = 0x0C2C
Asr_Ki_Addr = 0x0C2E
Asr_Kd_Addr = 0x0C30
Asr_Tau_Addr = 0x0C32
Apr_Kp_Addr = 0x0C34
Apr_Ki_Addr = 0x0C36
Asr_Kd_Addr = 0x0C38
Apr_Tau_Addr = 0x0C3A

Asr_Ctrl_Type = 0x06
Apr_Ctrl_Type = 0x04
Asr_Kp = 0.01
Asr_Ki = 0.5
Asr_Kd = 0
Asr_Tau = 0
Apr_Kp = 30
Apr_Ki = 0
Apr_Kd = 0
Apr_Tau = 0

-- 开始写入参数
Info("开始写入参数")

for i = 0, Slave_Mc_Num - 1, 1 do
    
    Info(string.format("写入夹爪%d速度环控制类型", i))
    ret = client:WriteUInt16(Slave_ID, Asr_Ctrl_Type_Addr+ (i * 0x64), 6)
    if ret == 0 then
        Info(string.format("写入夹爪%d速度环Kp成功", i))
    else
        Error(string.format("写入夹爪%d速度环Kp失败", i))
    end
    
     Info(string.format("写入夹爪%d位置环控制类型", i))
    ret = client:WriteUInt16(Slave_ID, Apr_Ctrl_Type_Addr+ (i * 0x64), 4)
    if ret == 0 then
        Info(string.format("写入夹爪%d速度环Kp成功", i))
    else
        Error(string.format("写入夹爪%d速度环Kp失败", i))
    end
	
    Info(string.format("写入夹爪%d速度环Kp", i))
    ret = client:WriteFloatCDAB(Slave_ID, Asr_Kp_Addr+ (i * 0x64), Asr_Kp)
    if ret == 0 then
        Info(string.format("写入夹爪%d速度环Kp成功", i))
    else
        Error(string.format("写入夹爪%d速度环Kp失败", i))
    end

    Info(string.format("写入夹爪%d速度环Ki", i))
    ret = client:WriteFloatCDAB(Slave_ID, Asr_Ki_Addr+ (i * 0x64), Asr_Ki)
    if ret == 0 then
        Info(string.format("写入夹爪%d速度环Ki成功", i))
    else
        Error(string.format("写入夹爪%d速度环Ki失败", i))
    end

    Info(string.format("写入夹爪%d位置环Kp", i))
    ret = client:WriteFloatCDAB(Slave_ID, Apr_Kp_Addr+ (i * 0x64), Apr_Kp)
    if ret == 0 then
        Info(string.format("写入夹爪%d位置环Kp成功", i))
    else
        Error(string.format("写入夹爪%d位置环Kp失败", i))
    end

    Info(string.format("写入夹爪%d位置环Ki", i))
    ret = client:WriteFloatCDAB(Slave_ID, Apr_Ki_Addr+ (i * 0x64), Apr_Ki)
    if ret == 0 then
        Info(string.format("写入夹爪%d位置环Ki成功", i))
    else
        Error(string.format("写入夹爪%d位置环Ki失败", i))
    end
    
    	Info(string.format("修改夹爪%d超前角系数", i))
    ret = client:WriteFloatCDAB(Slave_ID, 0x0C4A + (i * 0x64), 0.07)
    if ret == 0 then
        Info(string.format("修改夹爪%d超前角系数成功", i))
    else
        Error(string.format("修改夹爪%d超前角系数失败", i))
    end
	
	Info(string.format("保存夹爪%d参数", i))
    ret = client:WriteUInt16(Slave_ID, Device_Save_Addr + (i * 0x64), 1)
    if ret == 0 then
        Info(string.format("保存夹爪%d参数成功", i))
    else
        Error(string.format("保存夹爪%d参数失败", i))
    end

end

Info("执行完成")
--关闭串口
client:Disconnect()